// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Vector3Test.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;

    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Vector3 unit tests
    /// </summary>
    [TestClass]
    public class Vector3Test
    {
        /// <summary>
        /// Tests the default constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3DefaultConstructor()
        {
            Vector3 v = new Vector3();
            Assert.IsTrue(
                v.X == 0.0 && v.Y == 0.0 && v.Z == 0.0, "Vector3 Components are not zero in default constructor");
        }

        /// <summary>
        /// Tests the constructor with doubles
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3DoubleValuesConstructor()
        {
            Vector3 v = new Vector3(1.0, 2.0, 3.0);
            Assert.IsTrue(
                v.X == 1.0 && v.Y == 2.0 && v.Z == 3.0, "Vector3 Components are not what was passed into constructor");
        }

        /// <summary>
        /// Tests the copy constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3CopyConstructor()
        {
            Vector3 initialVector = new Vector3(1.0, 2.0, 3.0);
            Vector3 v = new Vector3(initialVector);
            Assert.IsTrue(v.X == 1.0 && v.Y == 2.0 && v.Z == 3.0, "Vector3 copy constructor failed");
        }

        /// <summary>
        /// Test vector axis methods
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3VectorAxis()
        {
            Vector3 v = Vector3.XAxis;
            Assert.IsTrue(v.X == 1.0 && v.Y == 0.0 && v.Z == 0.0, "Vector3 XAxis failed");

            v = Vector3.NegativeXAxis;
            Assert.IsTrue(v.X == -1.0 && v.Y == 0.0 && v.Z == 0.0, "Vector3 NegativeXAxis failed");

            v = Vector3.YAxis;
            Assert.IsTrue(v.X == 0.0 && v.Y == 1.0 && v.Z == 0.0, "Vector3 YAxis failed");

            v = Vector3.NegativeYAxis;
            Assert.IsTrue(v.X == 0.0 && v.Y == -1.0 && v.Z == 0.0, "Vector3 NegativeYAxis failed");

            v = Vector3.ZAxis;
            Assert.IsTrue(v.X == 0.0 && v.Y == 0.0 && v.Z == 1.0, "Vector3 ZAxis failed");

            v = Vector3.NegativeZAxis;
            Assert.IsTrue(v.X == 0.0 && v.Y == 0.0 && v.Z == -1.0, "Vector3 NegativeZAxis failed");
        }

        /// <summary>
        /// Tests the minus operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3MinusOperator()
        {
            Vector3 initialVector = new Vector3(1.0, 2.0, 3.0);
            Vector3 v = -initialVector;
            Assert.IsTrue(
                (Math.Abs(v.X + initialVector.X) < MathConstants.ErrorEpsilon)
                && (Math.Abs(v.Y + initialVector.Y) < MathConstants.ErrorEpsilon)
                && (Math.Abs(v.Z + initialVector.Z) < MathConstants.ErrorEpsilon), 
                "Vector3 minus operator failed");
        }

        /// <summary>
        /// Tests the addition operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3AdditionOperator()
        {
            Vector3 leftSide = new Vector3(10.0, 20.0, 30.0);
            Vector3 rightSide = new Vector3(1.0, 2.0, 3.0);
            Vector3 result = leftSide + rightSide;
            Assert.IsTrue(
                (Math.Abs(result.X - 11.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 22.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 33.0) < MathConstants.ErrorEpsilon), 
                "Vector3 addition operator failed");
        }

        /// <summary>
        /// Tests the subtraction operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3SubtractionOperator()
        {
            Vector3 leftSide = new Vector3(10.0, 20.0, 30.0);
            Vector3 rightSide = new Vector3(1.0, 2.0, 3.0);
            Vector3 result = leftSide - rightSide;
            Assert.IsTrue(
                (Math.Abs(result.X - 9.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 18.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 27.0) < MathConstants.ErrorEpsilon), 
                "Vector3 subtraction operator failed");
        }

        /// <summary>
        /// Tests scalar divide
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3ScalarDivide()
        {
            Vector3 initialVector = new Vector3(10.0, 20.0, 30.0);
            Vector3 result = initialVector / 2.0;
            Assert.IsTrue(
                (Math.Abs(result.X - 5.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 10.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 15.0) < MathConstants.ErrorEpsilon), 
                "Vector3 scalar divide operator failed");
        }

        /// <summary>
        /// Tests scalar multiply
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3ScalarMultiply()
        {
            Vector3 initialVector = new Vector3(10.0, 20.0, 30.0);
            Vector3 result = initialVector * 2.0;
            Assert.IsTrue(
                Math.Abs(result.X - 20.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 40.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 60.0) < MathConstants.ErrorEpsilon, 
                "Vector3 scalar multiply operator from right failed");
            result = 2.0 * initialVector;
            Assert.IsTrue(
                Math.Abs(result.X - 20.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Y - 40.0) < MathConstants.ErrorEpsilon
                && Math.Abs(result.Z - 60.0) < MathConstants.ErrorEpsilon, 
                "Vector3 scalar multiply operator from left failed");
        }

        /// <summary>
        /// Tests getting vector length
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3VectorLength()
        {
            Vector3 initialVector = new Vector3(1.0, 2.0, 3.0);
            double result = initialVector.Length();
            Assert.IsTrue(
                Math.Abs(result - Math.Sqrt(14.0)) < MathConstants.ErrorEpsilon, "Vector3 length calculation failed");
        }

        /// <summary>
        /// Tests vector normalization
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3VectorNormalization()
        {
            Vector3 result;
            result = Vector3.Normalize(new Vector3(2.0, 0.0, 0.0));
            Assert.IsTrue(
                (Math.Abs(result.X - 1.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon), 
                "Vector3 normalize X component failed");

            result = Vector3.Normalize(new Vector3(0.0, 2.0, 0.0));
            Assert.IsTrue(
                (Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 1.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon), 
                "Vector3 normalize Y component failed");

            result = Vector3.Normalize(new Vector3(0.0, 0.0, 2.0));
            Assert.IsTrue(
                (Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 1.0) < MathConstants.ErrorEpsilon), 
                "Vector3 normalize Z component failed");

            result = Vector3.Normalize(new Vector3(0.0, 0.0, 0.0));
            Assert.IsTrue(
                (Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon), 
                "Vector3 normalize zero length failed");
        }

        /// <summary>
        /// Tests vector normalization
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3NormalizeTest()
        {
            Vector3 vec = new Vector3(1, 1, 1);

            vec = Vector3.Normalize(vec);

            Assert.AreEqual(1, vec.Length());
        }

        /// <summary>
        /// Tests vector dot product
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3VectorDotProduct()
        {
            Vector3 left = new Vector3(1.0, 2.0, 3.0);
            Vector3 right = new Vector3(10.0, 20.0, 30.0);
            double result = Vector3.Dot(left, right);
            Assert.IsTrue(Math.Abs(result - 140.0) < MathConstants.ErrorEpsilon, "Vector3 Dot product failed");
        }

        /// <summary>
        /// Tests vector cross product
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3VectorCrossProduct()
        {
            Vector3 left = new Vector3(1.0, 0.0, 0.0);
            Vector3 right = new Vector3(0.0, 1.0, 0.0);
            Vector3 result = Vector3.Cross(left, right);
            Assert.IsTrue(
                (Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 1.0) < MathConstants.ErrorEpsilon), 
                "Vector3 Cross product X cross Y failed");

            left = new Vector3(0.0, 1.0, 0.0);
            right = new Vector3(0.0, 0.0, 1.0);
            result = Vector3.Cross(left, right);
            Assert.IsTrue(
                (Math.Abs(result.X - 1.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon), 
                "Vector3 Cross product Y cross Z failed");

            left = new Vector3(0.0, 0.0, 1.0);
            right = new Vector3(1.0, 0.0, 0.0);
            result = Vector3.Cross(left, right);
            Assert.IsTrue(
                (Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 1.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon), 
                "Vector3 Cross product Z cross X failed");

            left = new Vector3(0.0, 1.0, 0.0);
            right = new Vector3(1.0, 0.0, 0.0);
            result = Vector3.Cross(left, right);
            Assert.IsTrue(
                (Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z + 1.0) < MathConstants.ErrorEpsilon), 
                "Vector3 Cross product Y cross X failed");

            left = new Vector3(0.0, 0.0, 1.0);
            right = new Vector3(0.0, 1.0, 0.0);
            result = Vector3.Cross(left, right);
            Assert.IsTrue(
                (Math.Abs(result.X + 1.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon), 
                "Vector3 Cross product Z cross Y failed");

            left = new Vector3(1.0, 0.0, 0.0);
            right = new Vector3(0.0, 0.0, 1.0);
            result = Vector3.Cross(left, right);
            Assert.IsTrue(
                (Math.Abs(result.X - 0.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y + 1.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 0.0) < MathConstants.ErrorEpsilon), 
                "Vector3 Cross product X cross Z failed");
        }

        /// <summary>
        /// Test setting vector3 from vector4
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3SetFromVector4()
        {
            Vector4 initialVector = new Vector4(1.0, 2.0, 3.0, 4.0);
            Vector3 result = new Vector3();
            result.SetFromVector4(initialVector);
            Assert.IsTrue(
                (Math.Abs(result.X - 1.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Y - 2.0) < MathConstants.ErrorEpsilon)
                && (Math.Abs(result.Z - 3.0) < MathConstants.ErrorEpsilon), 
                "Vector3 conversion from Vector4 failed");
        }

        /// <summary>
        /// Tests the Hashcode method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3GetHashCode()
        {
            Vector3 vector = new Vector3();
            int result = vector.GetHashCode();
            Assert.IsTrue(result != 0, "Vector3 hash code is zero");
        }

        /// <summary>
        /// Tests the equality operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3EqualityOperator()
        {
            Vector3 vector1 = new Vector3(1.0, 2.0, 3.0);
            Vector3 vector2 = new Vector3(1.0, 2.0, 3.0);
            Assert.IsTrue(vector1 == vector2, "Vector3 should have been equal");
            vector2.X = 2.0;
            Assert.IsFalse(vector1 == vector2, "Vector3 should have been not equal in X component");
            vector2.X = 1.0;
            vector2.Y = 1.0;
            Assert.IsFalse(vector1 == vector2, "Vector3 should have been not equal in Y component");
            vector2.Y = 2.0;
            vector2.Z = 2.0;
            Assert.IsFalse(vector1 == vector2, "Vector3 should have been not equal in Z component");
            Assert.IsTrue(vector1 != vector2, "Vector3 != operator failed");
        }

        /// <summary>
        /// Tests the overridden equals method
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3EqualityMethod()
        {
            Vector3 vector1 = new Vector3(1.0, 2.0, 3.0);
            Vector3 vector2 = new Vector3(1.0, 2.0, 3.0);
            Assert.IsTrue(vector1.Equals(vector2), "Vector3 equals method failed");
            int foo = 0;
            Assert.IsFalse(vector1.Equals(foo), "Vector3 equals method retuned wrong result for unlike objects");
            vector2.X = 2.0;
            Assert.IsFalse(vector1.Equals(vector2), "Vector3 equals method failed in X component");
            vector2.X = 1.0;
            vector2.Y = 1.0;
            Assert.IsFalse(vector1.Equals(vector2), "Vector3 equals method failed in Y component");
            vector2.Y = 2.0;
            vector2.Z = 2.0;
            Assert.IsFalse(vector1.Equals(vector2), "Vector3 equals method failed in Z component");
        }

        /// <summary>
        /// Test the ToString operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3ToString()
        {
            Vector3 vector = new Vector3(1.0, 2.0, 3.0);
            string str = vector.ToString();

            // Not sure what the test should be
            Assert.IsTrue(str != null, "Vector3 tostring failed");
        }

        /// <summary>
        /// Test the DistanceFrom operator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3DistanceFrom()
        {
            Vector3 vector1 = new Vector3(1, 2, 3);
            Vector3 vector2 = new Vector3(1, 1, 1);

            Assert.IsTrue(
                Math.Abs(Math.Sqrt(5) - vector1.DistanceFrom(vector2)) < MathConstants.ErrorEpsilon, 
                "Distance is correct");
        }

        /// <summary>
        /// Test the interpolation for vector 3.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void Vector3Interpolate()
        {
            long startTime = 10;
            long endTime = 20;
            long[] desiredTimes = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20 };
            Vector3 start = new Vector3(1, 2, 3);
            Vector3 end = new Vector3(10, 20, 30);
            
            for (int i = 0; i < desiredTimes.Length; ++i)
            {
                double interpolationFactor = 1.0d * (desiredTimes[i] - startTime) / (endTime - startTime);
                Vector3 expected = new Vector3();
                expected.X = start.X + (end.X - start.X) * interpolationFactor;
                expected.Y = start.Y + (end.Y - start.Y) * interpolationFactor;
                expected.Z = start.Z + (end.Z - start.Z) * interpolationFactor;
                Vector3 interpolated = Vector3.LinearInterpolate(startTime, endTime, desiredTimes[i], start, end);
                Assert.AreEqual(expected.X, interpolated.X, MathConstants.ErrorEpsilon);
                Assert.AreEqual(expected.X, interpolated.X, MathConstants.ErrorEpsilon);
                Assert.AreEqual(expected.X, interpolated.X, MathConstants.ErrorEpsilon);
            }
        }

        /// <summary>
        /// Test the interpolation for vector 3.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentOutOfRangeException))]
        public void Vector3InterpolateEndEqualToStart()
        {
            long startTime = 10;
            long endTime = startTime;
            long[] desiredTimes = { 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20 };
            Vector3 start = new Vector3(1, 2, 3);
            Vector3 end = new Vector3(10, 20, 30);

            for (int i = 0; i < desiredTimes.Length; ++i)
            {
                double interpolationFactor = 1.0d * (desiredTimes[i] - startTime) / (endTime - startTime);
                Vector3 expected = new Vector3();
                expected.X = start.X + (end.X - start.X) * interpolationFactor;
                expected.Y = start.Y + (end.Y - start.Y) * interpolationFactor;
                expected.Z = start.Z + (end.Z - start.Z) * interpolationFactor;
                Vector3 interpolated = Vector3.LinearInterpolate(startTime, endTime, desiredTimes[i], start, end);
                Assert.AreEqual(expected.X, interpolated.X, MathConstants.ErrorEpsilon);
                Assert.AreEqual(expected.X, interpolated.X, MathConstants.ErrorEpsilon);
                Assert.AreEqual(expected.X, interpolated.X, MathConstants.ErrorEpsilon);
            }
        }
    }
}
